• 专利附录
  • 专利说明
  • 权利要求
  • 类似技术
  • 同族专利
  • 引用文献
  • 法律状态
  • 优先权
    • 专利摘要:

    公开号:WO1991019235A1
    申请号:PCT/JP1991/000751
    申请日:1991-05-31
    公开日:1991-12-12
    发明作者:Nobutoshi Torii；Ryo Nihei；Tetsuaki Kato
    申请人:Fanuc Ltd；
    IPC主号:G05B13-00
    专利说明:
    [0001] 明 細 書 コ ンプライ アンス制御方式 技 術 分 野
    [0002] 本発明はサーボ系のコ ンプライアンス制御方式に関し、 特 にロボッ ト等のサーボ系を制御するコ ンプライアンス制御方 式に関 ^る。 背 景 技 術
    [0003] 位置制御あるいは軌道制御のみを対象と している溶接、 塗 装ロボッ ト等は剛性が髙ぃほど、 望ま しい。 一方、 組立ロボ ッ ト等では、 単に剛性が高いのみでは実用性がない。 例えば、 ネジ締め等を考えると、 穴の位置及びロボッ トの位置が完全 に許容範囲に入っていれば問題ないが、 一般にこのようなこ とは望めない。 従って、 組立ロボッ ト等では一定の柔軟性が 要求される。
    [0004] このような目的のために、 剛性を任意に変更する方法とし て、 サ一ボ系に力を考慮したコ ンプライ アンス制御がある。
    [0005] しかし、 このサーボ系を一般の線形制御 （ P I制御） 等に よって実現しょうとすると制御対象の特性の変化などにより、 設定したコ ンプライ ア ンスを実現することが難しくなる。 す なわち、 一般の線形制御では、 制御対象のパラメ ータが完全 に既知でパラメ ータ変動がないとしてサーボ系を組んでいる ので、 大きなパラメ ータ変動のある制御対象に適用すること が困難であった。 発 明 の 開 示
    [0006] 本発明はこのような点に鑑みてなされたものであり、 パラ メ ータ変動 ·外乱等に強いスライディ ングモード制御によつ てコ ンプラ イ ア ンス制御を実現するコ ンプラ イ アンス制御方 式を提供するこ とを目的とする。
    [0007] 本発明では上記課題を解決するために、
    [0008] サ一ボ制御系を制御するコ ンプライアンス制御方式におい て、 前記サーボ制御系をスライディ ングモード制御し、 前記 スライディ ングモード制御の切り換え面を位置、 速度及び力 の偏差によって切り換えることにより、 任意のコ ンプライア ンスに設定可能なサーボ制御系を形成することを特徴とする コ ンプラ イ アンス制御方式が、 提供される。
    [0009] スライディ ングモード制御することにより制御対象のパラ メ 一タ変動があつても常に希望したコ ンプライアンスが得ら
    [0010] 4 る o 図 面 の 簡 単 な 説 明 第 1図はスライディ ングモー ド制御の処理のフローチヤ— 卜、
    [0011] 第 2図は本発明を実施するための口ボッ ト システムのハ一 ドウユアの構成図である。 発明を実施するための最良の形態 以下、 本発明の一実施例を図面に基づいて説明する。
    [0012] 第 2図は本発明を実施するためのロボッ ト システムのハ ー ドゥヱァの構成図である。 ホス トプロセッサ 9はロボッ ト全 体を制御するプロセッサである。 ホス トプロセッサ 9からは ロボッ トの位置指令が共有 R A M 1 0に書き込まれる。 なお、 ホス ト プロセ ッ サ 9 に結合される R O M、 R A M等は省略し "ある o
    [0013] ロボッ トに内蔵されたサーボモータ 2 2を制御する D S P (ディ ジタル ' シグナル * プロセ ッ サ） 1 1 は R 0 M 1 2 の システムプログラムに従って、 共有 R A M I 0の位置指令 rを一定時間ごとに読み取る。
    [0014] D S P 1 1 は、 この位置指令 r とサーボモータ 2 2に内 蔵されたパルスコーダ 2 3からの位置フィ一ドバック との 差分による位置偏差 εを求める。 また、 この位置偏差 εを微 分して、 速度偏差 ε ( 1 ) を計算する。 さらに、 教示者によつ て任意に与えられる ト ルク指令値 T c と図示されていないセ ンサからのフ ィ 一 ドノ ッ ク ト ルク T f を求め、 ト ルク偏差 ( T c - T f ) を計算する。 これらのデータから後述するよ うにスライディ ングモー ド制御を実行する。
    [0015] また、 スライディ ングモード制御によって求められる電流 補償ループの入力 トルク Tを計算し、 これからサーボモータ 2 2を駆動するための P W M波形を生成して、 D S L 1 4経 由でサーボアンプ 2 1 に送る。
    [0016] サーボアンプ 2 1 は P W M指令を受けて、 サーボモータ 2 2を駆動する。 サーボモータ 2 2は減速機を介して、 アーム 2 6を駆動する。
    [0017] 次に口ボッ トのモデルとして第 2図を例にスライディ ング モ一ド制御を説明する。
    [0018] T c ： ロボッ トか物体と接触して動作する場合の押しつけト ルク、 すなわち指令トルク
    [0019] T f ： 押しつけ力のフィードバッ ク トルク （センサにより測 定）
    [0020] Θ ： ロホッ ト tiifc
    [0021] e ： 位置偏差
    [0022] ε ( , ) ： 速度偏差
    [0023] するとプラ ン ト方程式は以下のように表すことができる。 J * <9 C 2 ) + T f = T
    [0024] また、 スライディ ングモード制御の切り換え関数 Sは、 以 下の式とする。
    [0025] S = ε い） + C * ε
    [0026] + ( 1 Κ ) * ( T c 一 T f )
    [0027] とする。 ここで、 入力 トルクを以下の様に置く、
    [0028] T = A * S + T 1
    [0029] ただし、 T 1 ： 切り換え入力
    [0030] A * S ：線形入力
    [0031] である。 ここで次の入力を与えると t (時間） が∞にて、 必 ず Sが 0に到達することが可能になる （証明は後述する） 。 ただし、
    [0032] A— m a κ
    [0033] と 。 Jma n ：プラ ン トの予想される最大ィナーシャ
    [0034] Jm i n ：プラ ン 卜の予想される最少ィ ナーシャ
    [0035] である
    [0036] 次に、
    [0037] T 1 -K 1 ( ε ) + Κ 2 (T c, T f )
    [0038] + K 3 (T f (') ) + Κ 4 ( r (2) )
    [0039] とする。 （ただし、 rは位置の指令値とする, )
    [0040] ここからは、 通常のスライディ ングモー ド制御と同じょう
    [0041] S≥ 0の時、 （切り換え入力て 1の選択する）
    [0042] K 1 ( ε ) の項に関して
    [0043] ε≥0 の時
    [0044] Κ 1 ( ε ) = — C 2 * Jmin * e
    [0045] ε < 0 の時
    [0046]
    [0047] K 2 (T c， T f ) の項に関して
    [0048] T c - T f ≥ 0の時
    [0049] K 2 (T c， T f ) =— Jmin * C * ( 1 /K)
    [0050] 氺 （T c一 T f ) + T f
    [0051] T c— T f < 0の時
    [0052] K 2 (T c， T f ) =— Jma« C * ( 1 ZK)
    [0053] * (T c - T f ) + T f
    [0054] K 3 (T f (') ) の項に関して
    [0055] T f ( 1 ) ≥ 0の時
    [0056] K 3 (T f (n ) =一 Jmi„ 氺 （ 1 ZK) * T f (1)
    [0057] T f Π) く 0の時
    [0058] Κ 3 (T f (1) ) =一 Jmax * ( 1 ZK)
    [0059] * T f (,}
    [0060] K 4 ( 0 τ ί2 ) の項に関して、
    [0061] Θ r (2) ≥ 0の時
    [0062] Κ 4 ( (2 )
    [0063]
    [0064] Θ r (2) < 0の時
    [0065] K 4 ( r (2) ) = Jmi„ * 0 (2) S < 0の時 （切り換え入力て 2の選択）
    [0066] K 1 ( ε ) の項に関して
    [0067] ε ≥0 の時
    [0068] Κ 1 ( ε ) = - C 2 * Jmi 氺 ε
    [0069] e < 0 の時
    [0070] K 1 ( ε ) = - C 2 * Jm * ε
    [0071] K 2 (Τ c， Τ f ) の項に関して
    [0072] T c - T f ≥ 0の時
    [0073] K 2 (T c， T f )
    [0074] =- Jma K * C * ( 1 ZK) * (T c Τ f )
    [0075] + T f
    [0076] T c - T f < 0の時
    [0077] K 2 (T c , Τ f )
    [0078] =一 Jm i n * C * ( 1 ZK) * (T c Τ f ) + T f
    [0079] K 3 (T f cn ) の項に関して、 T f ) ≥ 0の時
    [0080] Κ 3 (T f (') ) 一 Jma« * ( 1 ZK)
    [0081] 氺 T f (')
    [0082] T f ( ' ) < 0の時
    [0083] K 3 (T f (') ) 一 Jm in * ( 1 ZK)
    [0084] * T f (')
    [0085] K 4 ( r (2) ) の項に関して
    [0086] Θ r (2) ≥ 0の時
    [0087] K 4 ( r (2) ) = J mi n * r (2)
    [0088] Θ r (2) く 0の時
    [0089] Κ 4 ( r (2) ) = J maK * <9 r (2
    [0090] し 、
    [0091] S = 0
    [0092] に収束した状態を考える。 すなわち、 コ ンプライアンスがあ ることの証明を考える。 すなわち、
    [0093] ε (') + C * ε + ( 1 ZK) * (T c一 T f ) = 0 であり、 物体に接触している時を考えると、
    [0094] T f = T c + K * ( ε (') + C * e )
    [0095] となる。 すなわち、
    [0096] Kが小さければ T f ^ T cの力の制御が、
    [0097] Kが大きければ T f K * ( ε (Ι + C * e ) のバネ性の 強い制御が行える （K * Cがバネ定数、 Cが時定数） 。
    [0098] ロボッ 卜が対象に接触していない場合は、
    [0099] 一 T c =K * ( ε (1) + C * ε )
    [0100] を満足する位置で約り合って安定する。 T c - 0
    [0101] なら、
    [0102] e (') + C氺 ε = 0
    [0103] の力を制御しない位置の制御が行える。
    [0104] 次に S uf = 0に収束する証明について述べる。
    [0105] S = ε CI) + C * ε
    [0106] + ( 1 ZK) 水 (T c -T f )
    [0107] ( 1 )
    [0108] J * <9 (2) + Τ f =Τ ( 2 )
    [0109] (ブラ ン ト運動方程式）
    [0110] ただし、
    [0111] J ： ロボッ トのイナ一シャ （サーボモータのロータを含む） T ： 入力 トルク
    [0112] ： ロボッ トの位置
    [0113] Θ r (2) - θ (2) = ε (2) ( 3 )
    [0114] ここで、 リァプノ フ闋数として、
    [0115] V = ( S 2 Ζ 2 ) をとる。
    [0116] T = A * S +Τ 1 ( 4 )
    [0117] (T 1 ： 切り換え入力）
    [0118] こ こで、 （ 1 ) 式の両辺を微分すると、
    [0119] S ( 1 ) (2) + C氺 ε い）
    [0120] 一 ( 1 ΧΚ) 木 T f (')
    [0121] ( 5 )
    [0122] ( 2 ) 式に （ 3 ) 式を代入して整理すると、
    [0123] (2) 0 (2 ) + (T f / J ) 一 （TZ J ) ( 6 )
    [0124] ( 6 ) 式を （ 4 ) 、 （ 1 ) 式により変形すると、
    [0125] ε ( 2 ) = 0 τ + ( T f X J ) 一 ( 1 X J )
    [0126] 氺 {Α * ε (1) + A * C 氺 ε
    [0127] + Α氺 （ 1 /Κ) 氺 （T c 一 T f )
    [0128] + T 1 } ( 7 )
    [0129] ( 7 ) 式を （ 5 ) 式に代入して整理すると、
    [0130] S (') = {C - (A/ J ) } * ε (')
    [0131] - (A * CZ J ) * ε + (9 r ί2)
    [0132] + (T f / J )
    [0133] 一 {A/ ( J * K) } * (T c 一 T f ) 一 (T 1 / J ) 一 （ 1 ZK) * T f ('〕
    [0134] ( 8 )
    [0135] ( 1 ) 式を ε (1) について解き、 （ 8 ) 式に代入して整理す ると、
    [0136] S (, ) = {C一 （A/ J ) } * S - C 2 * ε
    [0137] - C * ( 1 ZK) * (T c 一 T f )
    [0138] + Γ r (2) + (T f J )
    [0139] 一 (T 1 X J ) 一 （ 1 ZK) * T f (n
    [0140] こ こで、 S ) を S倍すると、
    [0141] S * S
    [0142] = {C - (λ/ ) } * S 2
    [0143] 一 { C 2* ε + C * ( 1 ZK) 氺 （T c 一 T f )
    [0144] - (9 r (2 一 （T f Z J ) + (T 1 X J )
    [0145] + ( 1 XK) * T f ('〕 } * S ( 9 )
    [0146] ここで、 S * S (1) く 0を常に成立させるような入力を作る ( V (,) = S * S (,) < 0より極小値 0に V、 Sが収束する） 。
    [0147] C一 （ AZ J ) < 0
    [0148] A.— J m a. κ
    [0149] とすると、
    [0150] {C - (AX J ) } * S 2 < 0
    [0151] になる。 ここで、 （ 9 ) 式の右辺第 2項を負にすれば、
    [0152] V (,) = S * S (n < 0
    [0153] が常に成立する。 従って、 （ 9 ) 式の右辺第 2項が負になる ように T 1を選択すれば、 Sは 0に収束する。
    [0154] 第 1図はスライディ ングモード制御の処理のフローチャー トである。 図において、 S Pに続く数値はステップ審号を示 す。
    [0155] 〔 S P 1〕 共有 R AM 1 0より、 位置指令 ^ r、 トルク指令 T cを読み取る。
    [0156] C S Ρ 2 ] D S Ρ 1 1の内部レジスタから、 ロボッ トの位置 Θ フィ ー ドバッ ク トルク T f を読み取る。
    [0157] 〔 S P 3〕 上記のデータから、 位置偏差 ε、 速度偏差 ε (1) 、 トルク偏差 （T c一 T f ) を計算する。
    [0158] 〔 S P 4〕 先に述べた、 切り換え面 Sを以下の式から求める。 S = e (n + C * ε + ( 1 Κ) * (T c一 T f )
    [0159] C S P 5 ) S≥ 0なら S P 6へ、 そうでなければ S P ？へ進 む。
    [0160] 〔 S P 6〕 切り換え入力て 1を選択し、 それぞれの K 1 ( ε ) 、 Κ 2 (T c , T f ) 、 K 3 (T f (') ) 、 K 4 ( Θ r (2) ) の値を求め、 切り換え入力 T 1を求める。
    [0161] Τ 1 =Κ 1 ( e ) + K 2 (T c , T f )
    [0162] + K 3 (T f c,) ) + K 4 ( r (2) )
    [0163] 〔 S Ρ 7〕 切り換え入力 て 2を選択し、 S P 6 と同様に切り 換え入力 Τ 1を求める。
    [0164] 〔 S P 8〕 サーボアンプ 2 1への入力 Tを以下の式から求め る o
    [0165] T = A氺 S + T 1
    [0166] C S P 9 ] 電流補償ループへの入力の受け渡しを行う。
    [0167] このようにして、 スライディ ングモー ド制御によるコ ンプ ライアンス制御を行う ことにより、 ロボッ トの姿勢によって、 イナーシャ等が変化しても安定したコ ンプライ アンス制御が 可能になる。
    [0168] 上記の説明では口ボッ トのコ ンプライ アンス制御方式につ いて説明したが、 他のィナーシャ等が変動するサーボ系の制 御にも適用することができる。
    [0169] 以上説明したように本発明では、 イナーシャ変動の大きな 制御対象にスライディ ングモー ド制御を応用したコ ンプラィ ア ンス制御を行う ことより、 たとえイナ一シャ等変動がして もコ ンプラィ ァンスを設定した値に保てる口バス トな系が得 られる。
    权利要求:
    Claims 請 求 の 範 囲
    1. サーボ制御系を制御するコ ンプライアンス制御方式に おいて、
    前記サーボ制御系をスライディ ングモード制御し、 前記スライディ ングモ一ド制御の切り換え面を位置、 速度 及び力の偏差によって切り換えることにより、 任意のコ ンプ ラ ィ アンスに設定可能なサーボ制御系を形成する ことを特徵 とするコ ンプラ イ アンス制御方式。
    2. 前記切り換え面を以下の式、
    Sを切り換え面の値、 ε (1) を速度偏差、 εを位置偏差、 T cをトルク指令値、 T f を各関節でセ ンサ等により測定さ れるフィードバック トルクとし、
    S = ε (I) + C * ε + ( 1 ZK) * (T c一 T f )
    で行うことを特徴とする請求項 1記載のコ ンプライアンス制 御方式。
    3. 前記サーボ制御系はロボッ トのサーボ制御系であるこ とを特徴とする請求項 1記載のコ ンプライアンス制御方式。
    4. 前記ス ラ イディ ングモード制御はロボッ トのサ一ボ制 御系を制御するディ ジタル - シグナル ' プロセッサによって 制御することを特徴とする請求項 1記載のコ ンプライアンス 制御方式。
    类似技术:
    公开号 | 公开日 | 专利标题
    Lecours et al.2012|Variable admittance control of a four-degree-of-freedom intelligent assist device
    Ott et al.2010|Unified impedance and admittance control
    Jain et al.2013|Design of a model reference adaptive controller using modified MIT rule for a second order system
    Narendra1996|Neural networks for control theory and practice
    Togai et al.1985|Analysis and design of an optimal learning control scheme for industrial robots: A discrete system approach
    Al-Hiddabi et al.2002|Tracking and maneuver regulation control for nonlinear nonminimum phase systems: Application to flight control
    US5442270A|1995-08-15|Adaptive sliding mode control method for object of control including spring system
    Chung et al.2016|Motion control
    Kyriakopoulos et al.1988|Minimum jerk path generation
    Gazi2005|Swarm aggregations using artificial potentials and sliding-mode control
    US5499320A|1996-03-12|Extended task space control for robotic manipulators
    US5493631A|1996-02-20|Stabilized adaptive neural network based control system
    Reiner et al.1995|Robust dynamic inversion for control of highly maneuverable aircraft
    EP1290508B1|2005-03-30|Method, apparatus and design procedure for controlling multi-input, multi-output | parameter dependent systems using feedback lti&#39;zation
    US5101472A|1992-03-31|Military robotic controller with majorizing function and nonlinear torque capability
    Kosuge et al.1997|Control of a robot handling an object in cooperation with a human
    Hogan1985|Impedance control: An approach to manipulation: Part I—Theory
    Ho et al.2007|Robust fuzzy tracking control for robotic manipulators
    Walsh et al.1994|Stabilization of trajectories for systems with nonholonomic constraints
    EP0086950B1|1988-09-07|Method of controlling an industrial robot
    Luh1983|Conventional controller design for industrial robots—a tutorial
    KR960705267A|1996-10-09|예비산출용 이동제어 장치 및 방법|
    US5434489A|1995-07-18|Method and system for path planning in cartesian space
    Chen et al.2019|Nonlinear control of underactuated systems subject to both actuated and unactuated state constraints with experimental verification
    Moudgal et al.1994|Rule-based control for a flexible-link robot
    同族专利:
    公开号 | 公开日
    EP0486694A1|1992-05-27|
    EP0486694A4|1992-12-16|
    JPH0442302A|1992-02-12|
    引用文献:
    公开号 | 申请日 | 公开日 | 申请人 | 专利标题
    法律状态:
    1991-12-12| AK| Designated states|Kind code of ref document: A1 Designated state(s): US |
    1991-12-12| AL| Designated countries for regional patents|Kind code of ref document: A1 Designated state(s): DE IT SE |
    1992-01-20| WWE| Wipo information: entry into national phase|Ref document number: 1991910156 Country of ref document: EP |
    1992-05-27| WWP| Wipo information: published in national office|Ref document number: 1991910156 Country of ref document: EP |
    1994-02-21| WWW| Wipo information: withdrawn in national office|Ref document number: 1991910156 Country of ref document: EP |
    优先权:
    申请号 | 申请日 | 专利标题
    [返回顶部]